// @Author miaoqing
// @Date 2024/11/11 16:58:00
// @Desc
package logic

import (
	"core/command"
	"core/common"
	"core/component/function"
	"core/component/logger"
	"core/component/timer"
	"errors"
	"time"
)

type Robot struct {
	robotId     uint64
	handlers    map[command.Command]command.MsgHandler
	timer       *timer.Timer
	msgChan     chan interface{}
	done        chan int
	backpack    map[command.Command]*common.RepeatMsgData
	rosClient   *RosClient
	rosAddress  string
	reachedGoal chan bool
}

func newRobot(robotId uint64, rosAddress string) *Robot {
	robot := &Robot{
		robotId:     robotId,
		handlers:    make(map[command.Command]command.MsgHandler),
		timer:       timer.NewTimer(),
		msgChan:     make(chan interface{}, 64),
		done:        make(chan int, 0),
		backpack:    make(map[command.Command]*common.RepeatMsgData),
		rosAddress:  rosAddress,
		reachedGoal: make(chan bool),
	}
	robot.init()
	go robot.loop()
	logger.Infof("add a new robot %d", robotId)
	return robot
}

func (robot *Robot) loop() {
	ticker := time.NewTicker(50 * time.Microsecond)
	for {
		select {
		case <-ticker.C:
			robot.timer.Loop()
		case msg, ok := <-robot.msgChan:
			if ok {
				robot.onMessage(msg)
			}
		case result := <-robot.reachedGoal:
			logger.Infof("result  %v", result)
		case <-robot.done:
			robot.clear()
			return
		}
	}
}

func (robot *Robot) init() {
	robot.rosClient = newRosClient(robot)
	robot.registerMsgHandler()
	robot.registerTimer()
}

func (robot *Robot) clear() {
	robot.rosClient.Close()
	robot.rosClient = nil
	logger.Info("robot clear")
}

func (robot *Robot) robotClose() {
	robot.done <- 1
	WorldGetMe().removeRobotChan <- robot.robotId
}

func (robot *Robot) onMessage(msg interface{}) error {
	mRecv, ok := msg.(*common.MsgRecvData)
	if !ok {
		return errors.New("msg type error")
	}
	handler, ok := robot.handlers[command.Command(mRecv.Cmd)]
	if !ok {
		return errors.New("key is not exist")
	}
	handler(mRecv.Data)
	return nil
}

func (robot *Robot) sendMsg(cmd command.Command, msg interface{}) bool {
	msByte, err := function.MyMarshal(msg)
	if err != nil {
		return false
	}
	ret := clientManagerGetMe().sendMsgToClient(robot.robotId, int(cmd), msByte)
	if ret {
		rmd := &common.RepeatMsgData{
			Msg:      msg,
			SendTime: time.Now().Unix(),
		}
		robot.backpack[cmd] = rmd
	}
	return ret
}
